#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Sensor, S2,     Upper,          sensorSONAR)
#pragma config(Sensor, S3,     Lower,          sensorSONAR)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     CariageMotor,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an Tetrix robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.




void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


task main()
{
	ClearTimer(T1);
  PlayImmediateTone(1000, 20);
	wait1Msec(300);
	PlayImmediateTone(2000, 25);


  while(time10[T1] < 27000) //we keep going until we're at the bottom with less than 30s left.
	{
		motor[CariageMotor] = 100; //Since this is the Space Elevator Competition, it makes sense that we would start by going up.

		while((SensorValue[S2] > 35 || SensorValue[S2] < 0) )
		{
			wait1Msec(10);
		}

		motor[CariageMotor] = 100;

		wait1Msec(3000);

		motor[CariageMotor] = 80;

		wait1Msec(750);

		PlayImmediateTone(2000, 75);

		motor[CariageMotor] = 0;

		wait1Msec(1000);

		PlayImmediateTone(1000, 30);

		motor[CariageMotor] = -50;

		wait1Msec(1000);

		motor[CariageMotor] = -30;
/*
		while(SensorValue[S3] > 200)
		{
			wait1Msec(10);
		}
*/
		motor[CariageMotor] = 0;

		while(SensorValue[S3] > 50)
		{
			wait1Msec(10);
		}

		motor[CariageMotor] = -8;

		wait1Msec(500);
		PlayImmediateTone(2000, 20);
		wait1Msec(250);
		motor[CariageMotor] = 0;
		PlayImmediateTone(1000, 45);
		wait1Msec(9600);
		PlayImmediateTone(1750, 15);
		wait1Msec(175);
		PlayImmediateTone(2000, 22);
		wait1Msec(225);
	}
	while(true)
	{
		wait1Msec(325);
		PlayImmediateTone(1000, 250);
	}

	///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////


}
